#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "robot_tf_publisher");
  ros::NodeHandle n;

  ros::Rate r(100);

  tf::TransformBroadcaster broadcaster;
  tf::TransformBroadcaster broadcaster_kinect_rgb;
  tf::TransformBroadcaster broadcaster_kinect_depth;
  tf::Quaternion rot(-sqrt(0.5),0,0,sqrt(0.5));
  tf::Quaternion rot2(0,-sqrt(0.5),0,sqrt(0.5));
  tf::Quaternion rot_i(0, 0, 0, 1);
  tf::Quaternion final;
  final = rot;



  while(n.ok()){
//    broadcaster.sendTransform(
//      tf::StampedTransform(
//        tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.3, -0.35, 0.0)),
//        ros::Time::now(),"base_link", "laser"));
//    broadcaster_kinect_rgb.sendTransform(
//      tf::StampedTransform(
//        tf::Transform(final, tf::Vector3(0.17, 0.0, 0.90)),
//        ros::Time::now(),"base_link", "camera_rgb_optical_frame"));
    broadcaster_kinect_depth.sendTransform(
      tf::StampedTransform(
        tf::Transform(final, tf::Vector3(0.17, -0.15, 0.93)),
        ros::Time::now(),"base_link", "camera_link"));

    broadcaster_kinect_depth.sendTransform(
      tf::StampedTransform(
        tf::Transform(tf::Quaternion(1, 0, 0, 0), tf::Vector3(0.17, -0.15, 0.0)),
        ros::Time::now(),"base_link", "custom_scan"));
    /*broadcaster_kinect_depth.sendTransform(
      tf::StampedTransform(
        tf::Transform(tf::Quaternion(1, 0, 0, 0), tf::Vector3(0.0, -0.2, 0.0)),
                    ros::Time::now(),"map", "odom"));
    */
    r.sleep();
  }
}
